#include <iostream>
#include <boost/asio.hpp>
#include <boost/bind.hpp>
#include <thread>
#include "serial_kalman.h"
#include <chrono>
#include <iomanip>

// 这里将各种数据类型转为8位的字节
template <typename T>
std::vector<uint8_t> to_byte_vector(const T &value)
{
    std::vector<uint8_t> bytes(sizeof(T));
    std::memcpy(bytes.data(), &value, sizeof(T));
    return bytes;
}

int main(int argc, char *argv[])
{
    // 当主程序执行时，输入程序名时，后面也要输入IP地址，如：./serial_socket 127.0.0.1
    if (argc != 2)
    {
        std::cerr << "Usage: " << argv[0] << " <server_ip>" << std::endl;
        return 1;
    }

    std::string server_ip = argv[1];

    std::cout << " +----------------------------------+" << std::endl;
    std::cout << " |         MMW Measurement          |" << std::endl;
    std::cout << " +----------------------------------+" << std::endl;

    int mode;
    // 这里选择运行模式，模式1是只测速测距但是不发送，模式2会将测得的距离和速度值通过socket发送给服务端
    std::cout << "Select Mode (1 only measure; 2 measure and send): ";
    std::cin >> mode;
    // 这里的01 03 00 00 00 01 84 0A是测距指令，发送给雷达后，雷达会返回测距结果，解析就可以获得距离值
    std::vector<uint8_t> write_buffer = {0x01, 0x03, 0x00, 0x00, 0x00, 0x01, 0x84, 0x0A};
    // 这里构造函数中放的都是默认值，一般就这个输出化串口，与串口助手中一致
    SerialWrapper serial_wrapper("/dev/ttyUSB0", 115200, boost::asio::serial_port_base::character_size(8),
                                 boost::asio::serial_port_base::parity(boost::asio::serial_port_base::parity::none),
                                 boost::asio::serial_port_base::stop_bits(boost::asio::serial_port_base::stop_bits::one), 10);
    // 查看是否存在 data_client.csv 文件，如果不存在就创建，如果存在就不做什么。如果注销此行，就不会存再将数据保存到文件
    // serial_wrapper.ensure_file_exists("data_client.csv");  // Ensure data.csv exists

    if (serial_wrapper.open_serial())
    {
        if (mode == 1)
        {
            // Mode 1: Print measurements only
            auto empty_callback = [](int64_t, int, double, int){};
            // 分别打开三个线程，向串口写数据一个，读串口数据一个，将串口读到的数据来计算速度算一个
            std::thread send_thread(&SerialWrapper::write_data, &serial_wrapper, std::ref(write_buffer));
            std::thread receive_thread(&SerialWrapper::read_data, &serial_wrapper);
            std::thread speed_thread(&SerialWrapper::calculate_speed, &serial_wrapper, empty_callback);

            send_thread.join();    // Wait for the send thread to finish
            receive_thread.join(); // Wait for the receive thread to finish
            speed_thread.join();   // Wait for the speed calculation thread to finish
        }
        else if (mode == 2)
        {
            // Mode 2: Print and send data to server
            boost::asio::io_context io_context;
            boost::asio::ip::tcp::socket socket(io_context);
            // 传入客户端要连接的服务端的IP地址和端口号
            boost::asio::ip::tcp::endpoint endpoint(boost::asio::ip::address::from_string(server_ip), 502);

            bool is_connected = false;  // Connection status variable
            // 如果客户端没有连接上服务端，就不停地尝试连接
            while (!is_connected)
            {
                try
                {
                    socket.connect(endpoint);
                    is_connected = true; // Update status variable if connection is successful
                }
                catch (const std::exception &e)
                {
                    std::cerr << "Server not available, retrying..." << std::endl;
                    std::this_thread::sleep_for(std::chrono::seconds(1)); // Wait before retrying
                }
            }

            // Define the callback function
            auto speed_callback = [&socket, &is_connected, &endpoint](int64_t timestamp, int current_measurement, double speed, int dir)
            {
                // 如果客户端与服务端中途意外断开连接，就不进入这个回调函数中执行
                if (!is_connected) return; // Return immediately if not connected

                // Convert data to byte vectors
                auto timestamp_bytes = to_byte_vector(timestamp);
                auto measurement_bytes = to_byte_vector(current_measurement);
                auto speed_bytes = to_byte_vector(speed);
                auto dir_bytes = to_byte_vector(dir);

                // Combine all byte vectors into a single vector with fixed length
                // 全部转为字节通过socket发送。这里总共是24个字节，应该是时间戳占8个字节，距离值int占4个字节，速度值double占8个字节，方向值int占4个字节
                std::vector<uint8_t> message;
                message.insert(message.end(), timestamp_bytes.begin(), timestamp_bytes.end());
                message.insert(message.end(), measurement_bytes.begin(), measurement_bytes.end());
                message.insert(message.end(), speed_bytes.begin(), speed_bytes.end());
                message.insert(message.end(), dir_bytes.begin(), dir_bytes.end());

                boost::system::error_code error;
                boost::asio::write(socket, boost::asio::buffer(message), error);

                if (error)
                {
                    // 如果数据发送失败，就不停尝试重新连接服务端
                    std::cerr << "Error sending message: " << error.message() << std::endl;
                    socket.close(); // Close the socket before retrying
                    boost::asio::io_context io_context;
                    boost::asio::ip::tcp::socket new_socket(io_context);
                    socket = std::move(new_socket); // Replace old socket with new one

                    is_connected = false; // Update connection status variable

                    while (!is_connected)
                    {
                        try
                        {
                            socket.connect(endpoint);
                            is_connected = true; // Update status variable if connection is successful
                        }
                        catch (const std::exception &e)
                        {
                            std::cerr << "Server not available, retrying..." << std::endl;
                            std::this_thread::sleep_for(std::chrono::seconds(1)); // Wait before retrying
                        }
                    }
                }
                else
                {
                    std::cout << "Message sent to server: " << message.size() << " bytes" << std::endl;
                }
            };
            // 分别打开三个线程，向串口写数据一个，读串口数据一个，将串口读到的数据来计算速度算一个
            std::thread send_thread(&SerialWrapper::write_data, &serial_wrapper, std::ref(write_buffer));
            std::thread receive_thread(&SerialWrapper::read_data, &serial_wrapper);
            std::thread speed_thread(&SerialWrapper::calculate_speed, &serial_wrapper, speed_callback);

            send_thread.join();    // Wait for the send thread to finish
            receive_thread.join(); // Wait for the receive thread to finish
            speed_thread.join();   // Wait for the speed calculation thread to finish

            socket.close();
        }
        else
        {
            std::cerr << "Invalid mode selected." << std::endl;
        }
    }

    serial_wrapper.close();
    std::cout << "\nSerial communication ended" << std::endl;

    return 0;
}
